//Spring, Particle, Strut, SpatialPartition, and Box2d classes



final class Spring{
  private Particle particleA;
  private Particle particleB;
  private float restLength;
  private float forceConstant;
  private float friction;

  public Spring(Particle _pA,Particle _pB,float _restLength,float _forceConstant, float _friction){
//    debugCounter++;
    particleA = _pA; 
    particleB = _pB;
    restLength = _restLength;
    forceConstant = _forceConstant;
    friction = _friction;
  }
  public Spring(Particle _pA,Particle _pB,float _forceConstant,float _friction){
    this(_pA,_pB,_pA.distanceTo(_pB),_forceConstant,_friction);
  }
  public Spring(Particle _pA,Particle _pB,float _forceConstant){
    this(_pA,_pB,_pA.distanceTo(_pB),_forceConstant,defaultFriction);
  }

  public Spring(Particle _pA, Particle _pB){
    this(_pA,_pB,defaultForceConstant);
  }

  public void setRestLength(float newLength){
    restLength = newLength;
  }

  public float getRestLength(){
    return restLength;
  }

  public void setForceConstant(float newConstant){
    forceConstant = newConstant;
  }

  public float getForceConstant(){
    return forceConstant;
  }

  public void applyForce(){
    //225x per frame (with 5 steps per frame)
    particleA.setVel(); //for Verlet scheme, need to compute velocities before using them
    particleB.setVel();
    float springForce;
    Vector3d springf;
    float separation = particleA.distanceTo(particleB);
    if (separation > 0.0001){
      springForce = -forceConstant * (separation - restLength);
      springf = particleA.position.minus(particleB.position);
      springf.normalize();				
      Vector3d relVel = particleA.velocity.minus(particleB.velocity);
      float projection = -friction*relVel.dot(springf);
      springForce += projection;
      springf.multiply(springForce);
      particleA.forceAdd(springf);
      particleB.forceSubtract(springf);
    }
  }

}

//Utility function returns array of vectors (positions) from array of particles
Vector3d[] particlesToVectors(Particle[] parts){
//  debugCounter++;
  Vector3d result[] = new Vector3d[parts.length];
  for (int i=0; i< parts.length; i++){
    result[i] = parts[i].position;
  }
  return result;
}

ArrayList globalParticleList = new ArrayList();
int particleMax;

Particle getParticleByID(int _id){
  if(_id > particleMax){
    return null;
  }
  return (Particle)(globalParticleList.get(_id));
}


final class Particle{
  private boolean pinned;
  private Vector3d pinPos;
  public Vector3d velocity; //no velocity in a velocity-free Verlet scheme - need to specially init it!
  private float lastdelta; //stored for finding velocity
  public Vector3d force;
  public Vector3d position;	
  public Vector3d positionLast;
  public Vector3d truePositionLast; //the actual position at the previous time step - saved separately from positionLast, which may be altered to change velocity
  public float mass;
  public boolean isDrawn;
  private float radius;
  public Vector3d myColor;
  public float alphaValue;
  public boolean isCollided;
  private int id;
  private boolean lineDraw;
  private boolean isPlayer;
  private float airDrag;

  public int getID(){
    return id;
  }

  public void createID(){
    id = globalParticleList.size();
    //println(id);
    globalParticleList.add(this);
  }

  {
    alphaValue = 255.0; 
    pinned = false; 
    isCollided = true;
    id=-1;
    truePositionLast = new Vector3d(0,0,0);
    velocity = new Vector3d(0,0,0);
    lineDraw = false;
    isPlayer = false;
    airDrag = .3;
  }
  
  public void setAirDrag(float newDrag){
    airDrag = newDrag;
  }
  
  public void setPlayer(boolean isP){
    isPlayer = isP;
  }
  
  public boolean isPlayer(){
    return isPlayer;
  }

  public void setPinned(){
    pinned = true;
    pinPos = new Vector3d(position);
  }

  public void unsetPinned(){
    pinned = false;
  }

  public void constrainBox(Vector3d minVec, Vector3d maxVec){
    position.x = constrain(position.x,minVec.x,maxVec.x);
    position.y = constrain(position.y,minVec.y,maxVec.y);
    position.z = constrain(position.z,minVec.z,maxVec.z);
  }

  public Particle (Particle p){
    this(p.position,p.positionLast,p.mass,p.radius,p.myColor);
    alphaValue = p.alphaValue;
    pinned = p.pinned;
    //createID();
  }

  public Particle (Vector3d _position, Vector3d _positionLast, float m, float r, Vector3d col){
    position = new Vector3d(_position);
    positionLast = new Vector3d(_positionLast);
    force = new Vector3d(0,0,0);
    mass = m;
    isDrawn = true;
    radius = r;
    myColor = col;
    lastdelta = defaultDelta;
//    debugCounter++;
    //createID();
  }

  public Particle (Vector3d _position, float m, float r, Vector3d col){
    this(_position, _position, m, r, col);
  }

  public Particle (Vector3d _position, Vector3d _positionLast, float m){
    position =new Vector3d(_position);
    positionLast = new Vector3d(_positionLast);
    force = new Vector3d(0,0,0);
    mass = m;
    isDrawn = true;
    radius = defaultRadius;
    myColor = defaultColor;
    lastdelta = defaultDelta;
//    debugCounter++;
    //createID();
  }

  public Particle (Vector3d _pos, float m){
    position = new Vector3d(_pos);
    positionLast = new Vector3d(_pos);
    force = new Vector3d(0,0,0);
    mass = m;
    isDrawn = true;
    radius = defaultRadius;
    myColor = defaultColor;
    lastdelta = defaultDelta;
//    debugCounter++;
    //createID();
  }

  public void setLastDelta(float myDelta){
    lastdelta = myDelta;
  }

  public void setRadius(float newRad){
    radius = newRad;
  }

  public void setDraw(boolean tf){
    isDrawn = tf;
  }
  
  public void draw(float myAlpha){
    if (isDrawn){
      float factor = 1.0*max(width,height);
      if ( (-cameraX < (min(position.x,truePositionLast.x)-factor)) || (-cameraX > (max(position.x,truePositionLast.x)+factor) ) ){return;}
      if ( (-cameraY < (min(position.y,truePositionLast.y)-factor)) || (-cameraY > (max(position.y,truePositionLast.y)+factor) ) ){return;}
      if (lineDraw){
        //stroke(color(myColor.x,myColor.y,myColor.z,myAlpha));
        //line(position.x,position.y,truePositionLast.x,truePositionLast.y);
        beginShape(LINES);
        stroke(myColor.x,myColor.y,myColor.z,myAlpha);
        vertex(position.x,position.y);
        stroke(myColor.x,myColor.y,myColor.z,0);
        vertex(truePositionLast.x,truePositionLast.y);
        endShape();
      }else{
        fill(myColor.x,myColor.y,myColor.z,myAlpha);
        circle(position,radius);
      }
    }
  }

  public void draw(){
    if (isDrawn){
      float factor = 1.0*max(width,height);
      if ( (-cameraX < (min(position.x,truePositionLast.x)-factor)) || (-cameraX > (max(position.x,truePositionLast.x)+factor) ) ){return;}
      if ( (-cameraY < (min(position.y,truePositionLast.y)-factor)) || (-cameraY > (max(position.y,truePositionLast.y)+factor) ) ){return;}

      if (lineDraw){
//        stroke(color(myColor.x,myColor.y,myColor.z,alphaValue));
//        line(position.x,position.y,truePositionLast.x,truePositionLast.y);
        beginShape(LINES);
        stroke(color(myColor.x,myColor.y,myColor.z,alphaValue));
        vertex(position.x,position.y);
        stroke(color(myColor.x,myColor.y,myColor.z,1));
        vertex(truePositionLast.x,truePositionLast.y);
        endShape();
      }else{
        if (alphaValue < 254.9){ 
          fill(myColor.x,myColor.y,myColor.z,alphaValue);
        } else{
          fill(myColor.x,myColor.y,myColor.z);
        }
      
        circle(position,radius);	   
      }
    }
  }

  public void forceAdd(Vector3d toAdd){
    force.add(toAdd);
  }

  public void forceSubtract(Vector3d toSub){
    force.subtract(toSub);
  }

  public void forceZero(){
    force.set(0.0, 0.0, 0.0);
  }

  public void setVel(){ //this is naive right now - change to include force information
//    velocity = position.minus(positionLast).times(1.0/lastdelta);
    velocity.x = (position.x - positionLast.x) / lastdelta;
    velocity.y = (position.y - positionLast.y) / lastdelta;
  }

  public void timeStep(float delta){

    if (pinned){
      position.set(pinPos); 
      return;
    } else{
//          debugCounter++;
      //float airDrag = .3;
      setVel();
//      force.add(velocity.times(-airDrag*mass));
      force.x += velocity.x*(-airDrag*mass);
      force.y += velocity.y*(-airDrag*mass);

//      Vector3d accel = force.times(1.0/mass);
//      Vector3d buffer = new Vector3d(position);
      bufferA.set(force); bufferA.multiply(1.0/mass);
      bufferB.set(position);
      
      //remove this check eventually...
      if (Float.isNaN(bufferB.x)||Float.isNaN(bufferB.y)) {
        println("NaN encountered.");
        println(truePositionLast.x+"; "+truePositionLast.y);        
        throw new RuntimeException("NaN");
      }

//      position = position.times(2.0).minus(positionLast).plus(accel.times(delta*delta));//Verlet step - doesn't factor in lastdelta
      position.multiply(2.0);
      position.subtract(positionLast);
      bufferA.multiply(delta*delta);
      position.add(bufferA);
      
      positionLast.set(bufferB);
      truePositionLast.set(bufferB);
      forceZero();
      setLastDelta(delta);
    }
  }
  
  public void setLineDraw(){
    lineDraw = true;
  }

  public float distanceTo(Particle p){
    return position.distanceTo(p.position);
  }

}


ArrayList globalStrutList = new ArrayList();

Strut getStrutByID(int _id){
  return (Strut)(globalStrutList.get(_id));
}

final int NORMAL = 0;
final int PUSH = 1;
final int PULL = 2;

final class Strut{
  private Particle pA;
  private Particle pB;
  private float distance; //DON'T MAKE PUBLIC!  Need to also change distanceSqr to reset length!
  private float distanceSqr; //optimization variable
  private double tolerance = .001;//fractional tolerance
  private boolean draw;
  private boolean apply;
  private Vector3d strutColor;
  private int typeFlag;
  private int id;

  {
    strutColor = new Vector3d(155,155,155); 
    id=-1;
  }

  public int getStrutColor(){
    if (strutColor.x > 160 && strutColor.y < 160 && strutColor.z < 160){ 
      return REDINDEX; 
    }
    else if (strutColor.x < 160 && strutColor.y > 160 && strutColor.z < 160){ 
      return GREENINDEX; 
    }
    else if (strutColor.x < 160 && strutColor.y < 160 && strutColor.z > 160){ 
      return BLUEINDEX; 
    }
    return 0;
  }

  public int getID(){
    return id;
  }

  private void createID(){
    id = globalStrutList.size();
    //println(id);
    globalStrutList.add(this);
  }

  public Strut(Strut s){
    pA = s.pA;
    pB = s.pB;
    distance = s.distance;
    draw = s.draw;
    apply = s.apply;
    strutColor = s.strutColor;
    typeFlag = NORMAL;
    distanceSqr = distance*distance;
    //    createID();
  }

  public Strut(Particle A, Particle B, float d){
    pA = A;
    pB = B;
    distance = d;
    draw = true;
    apply = true;
    typeFlag = NORMAL;
    distanceSqr = distance*distance;
    //    createID();
  }

  public Strut(Particle A, Particle B, int colorIndex){
    pA = A;
    pB = B;
    distance = A.position.distanceTo(B.position);
    draw = true;
    apply = true;
    if (colorIndex == REDINDEX) strutColor = new Vector3d(255,0,0);
    else if (colorIndex == GREENINDEX) strutColor = new Vector3d(0,255,0);
    else if (colorIndex == BLUEINDEX) strutColor = new Vector3d(0,0,255);
    else strutColor = new Vector3d(150,150,150);
    typeFlag = PULL; //constructor only used for colored struts, which shouldn't push
        distanceSqr = distance*distance;
  }

  public Strut(Particle A, Particle B, int colorIndex, boolean drawMe){
    this(A,B,colorIndex);
    draw = drawMe;
  }

  public Strut(Particle A, Particle B){
    pA = A;
    pB = B;	
    distance = A.position.distanceTo(B.position);
        distanceSqr = distance*distance;
    draw = true;
    apply = true;
    typeFlag = NORMAL;
    //    createID();
  }

  public void setApply(boolean tf){
    apply = tf;
  }

  public boolean applyConstraint(){
    if (!apply) return true;
//        debugCounter++;
    float myDistSqr = pA.position.distanceSqr(pB.position);
    if (typeFlag == PULL && myDistSqr < distanceSqr) {
      return true;
    }
    else if (typeFlag == PUSH && myDistSqr > distanceSqr) {
      return true;
    }
//    if (myDistSqr < .001) println("Distance between points is zero.");
    float alpha = sqrt(distanceSqr/ (myDistSqr + .001)); //still works if distanceSqr == 0, since alpha is then zero
    float oneminusalpha = 1.0-alpha;
    if ( oneminusalpha < tolerance && oneminusalpha > -tolerance) {
      return true;
    }
    else {  //impose constraint
      float oneoversum = 1.0/(pA.mass + pB.mass);
      float cmx = (pA.mass*pA.position.x + pB.mass*pB.position.x) * oneoversum;
      float cmy = (pA.mass*pA.position.y + pB.mass*pB.position.y) * oneoversum;
      float cmz = (pA.mass*pA.position.z + pB.mass*pB.position.z) * oneoversum;
      pA.position.x = cmx*oneminusalpha + alpha*pA.position.x;
      pA.position.y = cmy*oneminusalpha + alpha*pA.position.y;
      pA.position.z = cmz*oneminusalpha + alpha*pA.position.z;
      pB.position.x = cmx*oneminusalpha + alpha*pB.position.x;
      pB.position.y = cmy*oneminusalpha + alpha*pB.position.y;
      pB.position.z = cmz*oneminusalpha + alpha*pB.position.z;
    }
    return false;
  }		

  public void setDraw(boolean drw){
    draw = drw;
  }

  public void draw(){
    if (!draw) return;
    float factor = 2*max(width,height);
    if ( (-cameraX < (min(pA.position.x,pB.position.x)-factor)) || (-cameraX > (max(pA.position.x,pB.position.x)+factor) ) ){
      return;
    }
    if ( (-cameraY < (min(pA.position.y,pB.position.y)-factor)) || (-cameraY > (max(pA.position.y,pB.position.y)+factor) ) ){
      return;
    }

    stroke(strutColor.x,strutColor.y,strutColor.z);
    //strokeWeight(4);
    if (drawing3d){
      line(pA.position.x,pA.position.y,pA.position.z,pB.position.x,pB.position.y,pB.position.z);
    }
    else{
      line(pA.position.x,pA.position.y,pB.position.x,pB.position.y);
    }
    noStroke();

  }

  public void setLength(float newLength){
    distance = newLength;
  }

  public Particle getPA(){
    return pA;
  }

  public Particle getPB(){
    return pB;
  }

  public void setPA(Particle A){
    pA = A;
  }

  public void setPB(Particle B){
    pB = B;
  }

}

final float edgeTolerance = 5.0; //add 5 pixels to each bin to catch edge cases
Box2d bufferBox = new Box2d(0,0,0,0);
Hashtable hashBuffer = new Hashtable();

final class SpatialPartition{
  private float resolution; //size of cell in one direction
  public float minX;
  public float maxX;
  public float minY;
  public float maxY;
  private int xCells;
  private int yCells;
  private ArrayList[] collidingWalls; //each element is an arraylist of walls that are in range
  //private SolidWall2d[][] collidingWalls; //change to array representation, since ArrayList has a lot of overhead

  private ArrayList[] staticCollidingStruts; //Store non-moving colliding struts separately for efficiency
  private ArrayList[] dynamicCollidingStruts;
  private Strut[] dcStrutList;

  private int indexNumber(int i, int j){
    int ind = xCells*j+i;
    if (ind < 0){ 
      return 0; 
    }
    else if (ind < xCells*yCells){ 
      return ind; 
    }
    else{ 
      return xCells*yCells-1; 
    }
    //This won't return a bad value, instead it just collapses everything outside valid ranges to the first or last grid points
  }

  private int iIndex(int num){//test this!!!
    return (num % xCells);
  }

  private int jIndex(int num){//test this!!!
    return ( (num - iIndex(num)) / xCells );
  }

  private int getCell(Vector3d vec){
    int myI = (int)Math.floor( (vec.x - minX) / resolution );
    int myJ = (int)Math.floor( (vec.y - minY) / resolution );
    if (myI < 0){
      myI = 0;
    }
    if (myI > xCells-1){
      myI = xCells-1;
    }
    if (myJ < 0){
      myJ = 0;
    }
    if (myJ > yCells-1){
      myJ = yCells-1;
    }
    return indexNumber(myI,myJ);
  }

  private Box2d getRange(int iStart, int iEnd, int jStart, int jEnd){
//        debugCounter++;
    float startX = minX + iStart * resolution; //from left edge of start box
    float endX = minX + (iEnd+1) * resolution; //to right edge of end box
    float startY = minY + jStart * resolution;
    float endY = minY + (jEnd+1) * resolution;
    return new Box2d( new Vector3d(startX, startY, 0), new Vector3d(endX, endY, 0) );
  }

  public SpatialPartition(float res,ArrayList myWalls,ArrayList staticStruts,ArrayList dynamicStruts){
    resolution = res;
    //Find min and max wall values in x and y coords
    minX = 0; 
    maxX = 0; 
    minY = 0; 
    maxY = 0;
    //Set mins/maxes to first element or zero, depending on whether first element exists
    if(myWalls.size()>0){
      minX = ((SolidWall2d)myWalls.get(0)).wallA.x; 
      maxX = minX; 
      minY = ((SolidWall2d)myWalls.get(0)).wallA.y; 
      maxY = minY; 
    }
    //First add walls
    for (int i=0; i<myWalls.size(); i++){
      SolidWall2d myWall = (SolidWall2d)myWalls.get(i);
      if (myWall.wallA.x < minX){
        minX = myWall.wallA.x;
      }
      if (myWall.wallB.x < minX){
        minX = myWall.wallB.x;
      }
      if (myWall.wallA.x > maxX){
        maxX = myWall.wallA.x;
      }
      if (myWall.wallB.x > maxX){
        maxX = myWall.wallB.x;
      }
      if (myWall.wallA.y < minY){
        minY = myWall.wallA.y;
      }
      if (myWall.wallB.y < minY){
        minY = myWall.wallB.y;
      }
      if (myWall.wallA.y > maxY){
        maxY = myWall.wallA.y;
      }
      if (myWall.wallB.y > maxY){
        maxY = myWall.wallB.y;
      }
    }
    //Now add static struts
    for (int i=0; i<staticStruts.size(); i++){
      Strut myStrut = (Strut)staticStruts.get(i);
      if (myStrut.pA.position.x < minX){
        minX = myStrut.pA.position.x;
      }
      if (myStrut.pB.position.x < minX){
        minX = myStrut.pB.position.x;
      }
      if (myStrut.pA.position.x > maxX){
        maxX = myStrut.pA.position.x;
      }
      if (myStrut.pB.position.x > maxX){
        maxX = myStrut.pB.position.x;
      }
      if (myStrut.pA.position.y < minY){
        minY = myStrut.pA.position.y;
      }
      if (myStrut.pB.position.y < minY){
        minY = myStrut.pB.position.y;
      }
      if (myStrut.pA.position.y > maxY){
        maxY = myStrut.pA.position.y;
      }
      if (myStrut.pB.position.y > maxY){
        maxY = myStrut.pB.position.y;
      }
    }      
    minX -= resolution; 
    maxX += resolution; 
    minY -= resolution; 
    maxY += resolution; //add an extra bin so that we don't land exactly on the edges with our outer walls - can lead to issues

    //Now find the appropriate numbers of cells, given the resolution and range - use ceil to err on the side of caution and add an extra cell if needed
    xCells = (int)Math.ceil((maxX - minX) / resolution);
    yCells = (int)Math.ceil((maxY - minY) / resolution);

    //Next we add the walls to the grid.  We use a slow but simple process to do this, as it should only happen upon map parsing, not durng simulation.
      //Note: this is really slow for large numbers of walls...
    //First allocate the array of ArrayLists to be xCells*yCells large.
    //Note: the (i,j) cell is going to be at index (xCells*j + i), as 1-d arrays tend to be a bit easier to use - use indexNumber(i,j) toget this, just to be safe
      collidingWalls = new ArrayList[xCells*yCells];
    for (int i=0; i<collidingWalls.length; i++){
      collidingWalls[i] = new ArrayList(); //allocate
      Box2d currentRange = getRange(iIndex(i),iIndex(i),jIndex(i),jIndex(i));
      currentRange.expand(edgeTolerance);
      for (int j=0; j<myWalls.size(); j++){
        // if wall pierces range, add it on to collidingWalls[i]
        SolidWall2d myWall = (SolidWall2d)myWalls.get(j);
        if (currentRange.hitBy(myWall.wallA, myWall.wallB)){ 
          collidingWalls[i].add(myWall); 
        }
      }
    }

    staticCollidingStruts = new ArrayList[xCells*yCells];
    for (int i=0; i<staticCollidingStruts.length; i++){
      staticCollidingStruts[i] = new ArrayList(); //allocate
      Box2d currentRange = getRange(iIndex(i),iIndex(i),jIndex(i),jIndex(i));
      currentRange.expand(edgeTolerance);
      for (int j=0; j<staticStruts.size(); j++){
        // if wall pierces range, add it on to collidingWalls[i]
        Strut myStrut = (Strut)staticStruts.get(j);
        if (currentRange.hitBy(myStrut.pA.position, myStrut.pB.position)){ 
          staticCollidingStruts[i].add(myStrut); 
        }
      }
    }
    
    dcStrutList = new Strut[dynamicStruts.size()];
    for (int i=0; i<dcStrutList.length; i++){
      dcStrutList[i] = (Strut)dynamicStruts.get(i);
    }
    dynamicCollidingStruts = new ArrayList[xCells*yCells];
    for (int i=0; i<dynamicCollidingStruts.length; i++){
      dynamicCollidingStruts[i] = new ArrayList(); //allocate
      Box2d currentRange = getRange(iIndex(i),iIndex(i),jIndex(i),jIndex(i));
      currentRange.expand(edgeTolerance);
      for (int j=0; j<dynamicStruts.size(); j++){
        // if wall pierces range, add it on to collidingWalls[i]
        Strut myStrut = (Strut)dynamicStruts.get(j);
        if (currentRange.hitBy(myStrut.pA.position, myStrut.pB.position)){ 
          dynamicCollidingStruts[i].add(myStrut); 
        }
      }
    }
  }
  
  public void refreshDynamicPartitioning(){
    //This is not as slow as the initial method, but it's definitely not optimal
    Strut buffer;
    for (int i=0; i<dcStrutList.length; i++){
      buffer = dcStrutList[i];
      //Check initial start/end cells
      float oldMinX = min( buffer.pA.truePositionLast.x, buffer.pB.truePositionLast.x );
      float oldMaxX = max( buffer.pA.truePositionLast.x, buffer.pB.truePositionLast.x );
      float oldMinY = min( buffer.pA.truePositionLast.y, buffer.pB.truePositionLast.y );
      float oldMaxY = max( buffer.pA.truePositionLast.y, buffer.pB.truePositionLast.y );  
      float newMinX = min( buffer.pA.position.x, buffer.pB.position.x );
      float newMaxX = max( buffer.pA.position.x, buffer.pB.position.x );
      float newMinY = min( buffer.pA.position.y, buffer.pB.position.y );
      float newMaxY = max( buffer.pA.position.y, buffer.pB.position.y );  
      int oldMinI = (int)Math.floor( (oldMinX - minX) / resolution );
      int oldMinJ = (int)Math.floor( (oldMinY - minY) / resolution );
      int oldMaxI = (int)Math.floor( (oldMaxX - minX) / resolution );
      int oldMaxJ = (int)Math.floor( (oldMaxY - minY) / resolution );
      int newMinI = (int)Math.floor( (newMinX - minX) / resolution );
      int newMinJ = (int)Math.floor( (newMinY - minY) / resolution );
      int newMaxI = (int)Math.floor( (newMaxX - minX) / resolution );
      int newMaxJ = (int)Math.floor( (newMaxY - minY) / resolution );
      oldMinI = constrainInt(oldMinI,0,xCells-1);
      oldMinJ = constrainInt(oldMinJ,0,yCells-1);
      oldMaxI = constrainInt(oldMaxI,0,xCells-1);
      oldMaxJ = constrainInt(oldMaxJ,0,yCells-1);
      newMinI = constrainInt(newMinI,0,xCells-1);
      newMinJ = constrainInt(newMinJ,0,yCells-1);
      newMaxI = constrainInt(newMaxI,0,xCells-1);
      newMaxJ = constrainInt(newMaxJ,0,yCells-1);
      
      for (int ai=minInt(oldMinI,newMinI); ai < maxInt(oldMaxI,newMaxI)+1; ai++){
        for (int aj=minInt(oldMinJ,newMinJ); aj < maxInt(oldMaxJ,newMaxJ)+1; aj++){
          //Remove from arraylist unless it's also in the new one
          int indexNum = dynamicCollidingStruts[indexNumber(ai,aj)].indexOf(buffer);
          if ( ai <= newMaxI && ai >= newMinI && aj <= newMaxJ && aj >= newMinJ ){
            //If indexNum == -1, then the strut is not already in our list, so we add it
            if (indexNum == -1) dynamicCollidingStruts[indexNumber(ai,aj)].add(buffer);
          } else{
            //If we're outside of the new range of indices, then if the strut is in the old list we remove it.
            //This check also means that we can take the first step with the more accurate method (which we've
            //really only done so that it's consistent with the other stuff)
            if (indexNum != -1) dynamicCollidingStruts[indexNumber(ai,aj)].remove( indexNum );
          }
        }
      }
    }
  }
  private int minInt(int i1,int i2){
    if (i1>i2) return i2;
    else return i1;
  }
  private int maxInt(int i1,int i2){
    if (i2>i1) return i2;
    else return i1;
  }
  private int constrainInt(int in,int inmin, int inmax){
    if (in < inmin) return inmin;
    if (in > inmax) return inmax;
    return in;
  }

  public Strut[] getStrutsInRange(Box2d _range){
//    debugCounter++;
    Hashtable hashResult = hashBuffer; //use a hashtable so we don't have dupes - though we do pay an efficiency penalty, since the Java Hashtable class is synchronized...
    hashBuffer.clear();
    int beginCellIndex = getCell(_range.minVec);
    int endCellIndex = getCell(_range.maxVec);
    int iBegin = iIndex(beginCellIndex);
    int jBegin = jIndex(beginCellIndex);
    int iEnd = iIndex(endCellIndex);
    int jEnd = jIndex(endCellIndex);
    for (int i=iBegin; i<iEnd+1; i++){
      for (int j=jBegin; j<jEnd+1; j++){
        int currIndex = indexNumber(i,j);
        for (int k=0; k<staticCollidingStruts[currIndex].size(); k++){
          Strut currStrut = (Strut)staticCollidingStruts[currIndex].get(k);
          hashResult.put("strut_"+currStrut.id,currStrut);
        }
        for (int k=0; k<dynamicCollidingStruts[currIndex].size(); k++){
          Strut currStrut = (Strut)dynamicCollidingStruts[currIndex].get(k);
          hashResult.put("strut_"+currStrut.id,currStrut);
        }          
      }
    }
    Object[] oresult = (hashResult.values().toArray());
    Strut[] result = new Strut[oresult.length];
    for (int i=0; i<oresult.length; i++){
      result[i] = (Strut)oresult[i];
    }
    return result;
  }

  public Strut[] getStrutsInRange(float x1,float y1,float x2, float y2){
//        debugCounter++;
    bufferBox.set(x1,y1,x2,y2);
    return getStrutsInRange(bufferBox);
  }


  public SolidWall2d[] getWallsInRange(Box2d _range){
    //debugCounter++;
    Hashtable hashResult = hashBuffer; //use a hashtable so we don't have dupes - though we do pay an efficiency penalty, since the Java Hashtable class is synchronized...
    hashBuffer.clear();
    int beginCellIndex = getCell(_range.minVec);
    int endCellIndex = getCell(_range.maxVec);
    int iBegin = iIndex(beginCellIndex);
    int jBegin = jIndex(beginCellIndex);
    int iEnd = iIndex(endCellIndex);
    int jEnd = jIndex(endCellIndex);
    for (int i=iBegin; i<iEnd+1; i++){
      for (int j=jBegin; j<jEnd+1; j++){
        int currIndex = indexNumber(i,j);
        for (int k=0; k<collidingWalls[currIndex].size(); k++){
          SolidWall2d currWall = (SolidWall2d)collidingWalls[currIndex].get(k);
          hashResult.put("wall_"+currWall.idNum,currWall);
        }
      }
    }
    Object[] oresult = (hashResult.values().toArray());
    SolidWall2d[] result = new SolidWall2d[oresult.length];
    for (int i=0; i<oresult.length; i++){
      result[i] = (SolidWall2d)oresult[i];
      if (debugFlag) {
        pushMatrix();
        translate(cameraX,cameraY);
        result[i].draw(REDVEC);
        result[i].skipNextDraw();
        popMatrix();
      }
    }
    return result;
  }

  public SolidWall2d[] getWallsInRange(float x1,float y1,float x2, float y2){
//        debugCounter++;
    bufferBox.set(x1,y1,x2,y2);
    return getWallsInRange(bufferBox);
  }

  public void draw(){
    int initI = iIndex(getCell(new Vector3d(-cameraX,-cameraY,0)));
    int initJ = jIndex(getCell(new Vector3d(-cameraX,-cameraY,0)));
    int endI = iIndex(getCell(new Vector3d(-cameraX+width,-cameraY+height,0)));
    int endJ = jIndex(getCell(new Vector3d(-cameraX+width,-cameraY+height,0)));
    Box2d test = getRange(initI,initI,initJ,initJ);
    Box2d test2 = getRange(initI+1,initI+1,initJ+1,initJ+1);
    for (int i=initI; i<endI+1; i++){
      for (int j=initJ; j<endJ+1; j++){
        Box2d myBox = getRange(i,i,j,j);
        int currIndex = indexNumber(i,j);
        if(dynamicCollidingStruts[currIndex].size() > 0 ){
        //collidingWalls[currIndex].size() + staticCollidingStruts[currIndex].size() + dynamicCollidingStruts[currIndex].size() > 0 ){
          fill(255,255,255,100);
        }
        myBox.draw();
        noFill();
      }
    }
  }

}

final class Box2d{
  public Vector3d minVec;
  public Vector3d maxVec;

  public Box2d(Vector3d _min, Vector3d _max){
//    debugCounter++;
    minVec = new Vector3d(_min.x, _min.y,0);
    maxVec = new Vector3d(_max.x, _max.y,0);
    if (minVec.x > maxVec.x) {
      minVec.x = maxVec.x; 
      maxVec.x = _min.x;
    }
    if (minVec.y > maxVec.y) {
      minVec.y = maxVec.y; 
      maxVec.y = _min.y;
    }
  }
  
  public Box2d(float x1,float y1, float x2, float y2){
//    debugCounter++;
    minVec = new Vector3d(x1, y1,0);
    maxVec = new Vector3d(x2, y2,0);
    if (minVec.x > maxVec.x) {
      minVec.x = maxVec.x; 
      maxVec.x = x1;
    }
    if (minVec.y > maxVec.y) {
      minVec.y = maxVec.y; 
      maxVec.y = y1;
    }
  }
  
  public void set(float x1, float y1, float x2, float y2){
    minVec.set(x1,y1,0);
    maxVec.set(x2,y2,0);
    if (minVec.x > maxVec.x) {
      minVec.x = maxVec.x; 
      maxVec.x = x1;
    }
    if (minVec.y > maxVec.y) {
      minVec.y = maxVec.y; 
      maxVec.y = y1;
    }
  }
  
  public void expand(float amt){
    //Push out all edges by amt - useful for catching edge cases
    minVec.x -= amt;
    minVec.y -= amt;
    maxVec.x += amt;
    maxVec.y += amt;
  }

  public boolean contains(Vector3d test){
    //Does it contain the point "test"?
    if (test.x > minVec.x && test.x < maxVec.x &&
      test.y > minVec.y && test.y < maxVec.y) {
      return true;
    }
    else{ 
      return false;
    }
  }

  public boolean hitBy(Vector3d lineA, Vector3d lineB){
    //Is it pierced by the line segment between lineA and lineB?
    //First some early exit stuff
    if (lineA.x>maxVec.x && lineB.x > maxVec.x){
      return false;
    }
    if (lineA.x<minVec.x && lineB.x < minVec.x){
      return false;
    }
    if (lineA.y>maxVec.y && lineB.y > maxVec.y){
      return false;
    }
    if (lineA.y<minVec.y && lineB.y < minVec.y){
      return false;
    }
    if (contains(lineA) || contains(lineB)){
      return true;
    } //quick test
    else if (lineCross(lineA,lineB,minVec,maxVec) || lineCross(lineA,lineB, new Vector3d(minVec.x-EPSILON,maxVec.y+EPSILON,0),new Vector3d(maxVec.x+EPSILON,minVec.y-EPSILON,0) ) ){
      return true;
    } //detailed test
    else{ 
      return false; 
    } //no pierce
  }

  public void draw(){
    stroke(255);
    rect(minVec.x,minVec.y,(maxVec.x-minVec.x),(maxVec.y-minVec.y));
  }

}
